#include <ros/ros.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Imu.h>
#include <boost/foreach.hpp>
#include <ros/time.h>

#include <pcl/common/common.h>
#include <pcl/common/eigen.h>
#include <pcl/common/transforms.h>
#include <pcl/features/normal_3d.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/transformation_estimation_point_to_plane.h>
#include <pcl_conversions/pcl_conversions.h>

namespace velodyne_pcl
{
        struct PointXYZIRT
        {
                PCL_ADD_POINT4D;                // quad-word XYZ
                float intensity;                ///< laser intensity reading
                uint16_t ring;                  ///< laser ring number
                float time;                     ///< laser time reading
                EIGEN_MAKE_ALIGNED_OPERATOR_NEW // ensure proper alignment
        } EIGEN_ALIGN16;
} // namespace velodyne_pcl

POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_pcl::PointXYZIRT, (float, x, x) //
                                  (float, y, y)                            //
                                  (float, z, z)                            //
                                  (float, intensity, intensity)            //
                                  (uint16_t, ring, ring)                   //
                                  (float, time, time)                      //
)

typedef velodyne_pcl::PointXYZIRT RTPoint;
typedef pcl::PointCloud<RTPoint> RTPointCloud;

std::string input_bag_path, output_bag_path;
std::string lidar_topic, imu_topic, lidar_data_type;

int main(int argc, char **argv)
{
        ros::init(argc, argv, "modify_pointcloud_timestamps");
        ros::NodeHandle nh;

        nh.param<std::string>("input_bag_path", input_bag_path, "");
        nh.param<std::string>("output_bag_path", output_bag_path, "");
        nh.param<std::string>("lidar_topic", lidar_topic, "");
        nh.param<std::string>("imu_topic", imu_topic, "");
        nh.param<std::string>("lidar_data_type", lidar_data_type, "");

        // std::cout << input_bag_path << std::endl;
        // std::cout << output_bag_path << std::endl;

        if (1)
        {
                // 读取ROS bag
                rosbag::Bag bag;
                bag.open(input_bag_path, rosbag::bagmode::Read);

                // 创建输出的ROS bag
                rosbag::Bag output_bag;
                output_bag.open(output_bag_path, rosbag::bagmode::Write);

                // 构造rosbag view，它是一个rosbag中所有消息的集合
                rosbag::View view(bag);

                // 定义偏移量
                // ros::Duration offset(10.0); // 10秒

                int lidar_nums = 0, imu_nums = 0;
                double start_point_time = 0; // 第一个点的时间戳
                // 遍历所有消息，只修改点云消息的时间戳
                BOOST_FOREACH (rosbag::MessageInstance const m, view)
                {
                        if (m.getTopic() == lidar_topic) // 处理雷达
                        {
                                if (m.getDataType() == lidar_data_type)
                                {
                                        lidar_nums++;
                                        sensor_msgs::PointCloud2ConstPtr cloud_msg = m.instantiate<sensor_msgs::PointCloud2>();
                                        double end_point_time = cloud_msg->header.stamp.toSec(); // 记录最后一个点的时间戳

                                        RTPointCloud::Ptr cur_cloud(new RTPointCloud());  // 修改前的帧
                                        RTPointCloud::Ptr raw_cloud_(new RTPointCloud()); // 修改后的帧

                                        raw_cloud_->header = cur_cloud->header;
                                        raw_cloud_->height = cur_cloud->height;
                                        raw_cloud_->is_dense = cur_cloud->is_dense;

                                        pcl::fromROSMsg(*cloud_msg, *cur_cloud);

                                        // 修改点云的时间戳
                                        for (size_t i = 0; i < cur_cloud->size(); i++)
                                        {
                                                start_point_time = end_point_time + cur_cloud->points[0].time;       // 帧头的时间戳
                                                double point_timestamp = end_point_time + cur_cloud->points[i].time; // 每个点的时重新计算间戳
                                                RTPoint point = cur_cloud->points[i];
                                                point.time = point_timestamp - start_point_time;
                                                raw_cloud_->push_back(point);
                                                // if (start_point_time < point_timestamp) // 找当前帧中时间戳最小的一个点，作为当前帧的时间戳
                                                //         start_point_time = point_timestamp;
                                        }

                                        ros::Time scan_time = ros::Time().fromSec(start_point_time);
                                        // raw_cloud_->header.stamp = ros::Time().fromSec(start_point_time);
                                        raw_cloud_->header.stamp = scan_time.toNSec() / 1e3;

                                        sensor_msgs::PointCloud2 source_msg;
                                        pcl::toROSMsg(*raw_cloud_, source_msg);
                                        output_bag.write(lidar_topic, scan_time, source_msg);

                                        // ros::Time new_time = old_time + offset;
                                        // cloud->header.stamp = new_time;

                                        // 将修改后的点云写入输出的ROS bag
                                        // output_bag.write("cloud", new_time, *cloud);
                                }
                        }
                        else if (m.getTopic() == imu_topic) // 处理imu
                        {
                                imu_nums++;
                                sensor_msgs::Imu::ConstPtr imu_msg = m.instantiate<sensor_msgs::Imu>();
                                ros::Time imu_time = imu_msg->header.stamp;
                                output_bag.write(imu_topic, imu_time, *imu_msg);
                        }
                        else
                                continue;
                        std::cout << lidar_nums << "   " << imu_nums << "   " << std::setprecision(19) << start_point_time << std::endl;
                }

                bag.close();
                output_bag.close();
        }

        return 0;
}
